/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "motor.h"
#include "pid.h"
#include "stdio.h"
#include "string.h"
#include "encoder.h"
#include "ti/driverlib/dl_uart_main.h"
#include "ti/driverlib/m0p/dl_core.h"
#include "ti/driverlib/m0p/sysctl/dl_sysctl_mspm0g1x0x_g3x0x.h"
#include "ti_msp_dl_config.h"
#include "board.h"
#include "bsp_mpu6050.h"
#include "inv_mpu.h"
#include "ultrasonic.h"
#include "pid.h"
#include "mycontrol.h"
#include "motor.h"
#include "Tracking.h"
#include "oled.h"
#include "GUI.h"
#include "tjc_hmi.h"

void TE_SendString(char *str);
void USART_PID_Adjust(uint8_t Motor_n);

char OledString[30];
int y = 0;
int modenow = 0; //当前状态机状态
int timeout = 0;
uint8_t openmv_data[15] = {0}; //接收到的OPENMV数据
int RT[15] = {0};
int laps =0;
volatile uint8_t gEchoData = 0; //接收到的电脑串口数据
volatile uint8_t x = 1;
char TE_data[30] = {0}; //发送给串口的数据
uint8_t ret = 1;
float pitch=0,roll=0,yaw=0; //MPU6050测量的当前角度值
uint8_t status;
uint8_t dist; //获取超声波检测的距离
uint8_t RxBuffer[1];//串口接收缓冲
uint16_t RxLine = 0;//指令长度
uint8_t DataBuff[200];//指令内容
int yile = 0;
uint8_t HMI_RxBuffer[1];//串口接收缓冲
uint16_t HMI_RxLine = 0;//指令长度
uint8_t HMI_DataBuff[3];//指令内容

int main(void) {
  SYSCFG_DL_init(); 

    
//初始化串口0中断
    NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
    NVIC_EnableIRQ(UART_0_INST_INT_IRQN);

//初始化串口1中断
    NVIC_ClearPendingIRQ(UART_1_INST_INT_IRQN);
    NVIC_EnableIRQ(UART_1_INST_INT_IRQN);

//初始化串口2中断
    //NVIC_ClearPendingIRQ(UART_2_INST_INT_IRQN);
    //NVIC_EnableIRQ(UART_2_INST_INT_IRQN);

//初始化串口3中断
    NVIC_ClearPendingIRQ(UART_3_INST_INT_IRQN);
    NVIC_EnableIRQ(UART_3_INST_INT_IRQN);

//初始化MPU6050
    //MPU6050_Init();
    //mpu_dmp_init();

// 初始化OLED
    //OLED_Init();

//开启PWM计数
  DL_TimerG_startCounter(MOTOR_PWM_INST);
  DL_TimerG_startCounter(DUOJI_PWM_INST);

//初始化定时器0中断（主循环定时器）
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
//使能定时器0开始计时
    DL_TimerA_startCounter(TIMER_0_INST);
    
//初始化编码器中断
    NVIC_EnableIRQ(GPIO_Encoder_INT_IRQN);

//初始化MPU6050的i2c
    //NVIC_EnableIRQ(I2C_MPU6050_INST_INT_IRQN);

//初始化超声波
    NVIC_EnableIRQ(TIMER_Ultrasonic_INST_INT_IRQN);

//初始化pid
  PID_param_init();

  while (1) 
  {
    y++;
    if (y >= 800)
    y = 0;
    // printf("hello");
    //  DL_GPIO_togglePins(GPIO_LED_PORT,
    //                     GPIO_LED_LED_PIN);      
    //sprintf(OledString, "hyt is genius!  ");
    //OLED_ShowString(0, 0, (u8 *)OledString, 16);

    //向串口发送测试数据
    sprintf(TE_data,"speed1:%f\r\n",speed1);
    TE_SendString(TE_data);
    //sprintf(TE_data,"speed2:%f\r\n",Pid_speed2);
    //TE_SendString(TE_data);
    delay_ms(200);

    //向串口屏打印数据
    //printf("main.sp1.txt=%.1f\xff\xff\xff",Pid_speed1);
    //printf("main.sp2.txt=%d\xff\xff\xff",y);
    //Tjc_Printf("page0.t0.txt=\"%s\"","hyt");
    //屏幕发送数据
    // sprintf(TE_data,"%.2f",-Pid_speed1);
    // Tjc_Printf("page0.t4.txt=\"%s\"",TE_data);
    // sprintf(TE_data,"%.2f",Pid_speed2);
    // Tjc_Printf("page0.t5.txt=\"%s\"",TE_data);
    //向oled打印数据

    // sprintf(OledString, "hyt ");
    // OLED_ShowString(60, 4, (uint8_t *)OledString, 16);
    // sprintf(OledString, "y=%d  ", y);
    // OLED_ShowString(0, 2, (uint8_t *)OledString, 16);
    //menu_show ();

    //sprintf(OledString, "XL=%d  ", RT[0]);
    //OLED_ShowString(0, 4, (uint8_t *)OledString, 16);


    //MPU6050获取目标角度数据（待测）
   // status = mpu_dmp_get_data(&pitch,&roll,&yaw);//返回值为0则成功，否则失败，不获取数据
    //if( status == 0 )
   // { 
    //    printf("pitch =%d\r\n", (int)pitch);
   //     printf("roll =%d\r\n", (int)roll);
   //    printf("yaw =%d\r\n\r\n", (int)yaw);
   // }
    //delay_ms(20);

    //键盘搜索
   // if (KEY_Scan(1) == 1) {
   //   modenow1 = 1;
   // }
    //Motor_Set(y,y);

    //超声波模块(100ms测量一次)
    //dist = getUltrasonicDist();
    //delay_ms(100);
    //if(!dist)
    //    continue;
    //printf("Dist: %u cm\r\n", dist);
  }

    
}



//void UART_0_INST_IRQHandler(void)
//{
//    switch (DL_UART_Main_getPendingInterrupt(UART_0_INST))
//    {
 //       case DL_UART_MAIN_IIDX_RX:
 //           gEchoData = DL_UART_Main_receiveData(UART_0_INST);
 //           DL_UART_Main_transmitData(UART_0_INST,gEchoData);
 //           break;
 //       default:
 //           break;
 //   }
//}

void UART_0_INST_IRQHandler(void)//chuankoutiaocan
{
    switch (DL_UART_Main_getPendingInterrupt(UART_0_INST))
    {
        case DL_UART_MAIN_IIDX_RX:
        {
            RxLine++;                      //每接收到一个数据，进入回调数据长度加1
            RxBuffer[0] = DL_UART_Main_receiveData(UART_0_INST);//接收目标数据
            DataBuff[RxLine-1]=RxBuffer[0];  //把每次接收到的数据保存到缓存数组
            if(RxBuffer[0]==0x21)            //接收结束标志位，这个数据可以自定义，根据实际需求，这里只做示例使用，不一定是0x21
            {
                USART_PID_Adjust(2);//数据解析和参数赋值函数
                memset(DataBuff,0,sizeof(DataBuff));  //清空缓存数组
                RxLine=0;  //清空接收长度
            }
            RxBuffer[0]=0;
        }
            break;
        default:
            break;
    }
}

void UART_1_INST_IRQHandler(void)//chuankoutiaocan
{
    switch (DL_UART_Main_getPendingInterrupt(UART_1_INST))
    {
        case DL_UART_MAIN_IIDX_RX:
        {
            
            HMI_RxLine++;
            HMI_RxBuffer[0] = DL_UART_Main_receiveData(UART_1_INST);//接收目标数据
            HMI_DataBuff[HMI_RxLine-1]=HMI_RxBuffer[0];  //把每次接收到的数据保存到缓存数组
            if(HMI_DataBuff[HMI_RxLine-3]==0x0A && HMI_DataBuff[HMI_RxLine-1]==0xFF)            //接收结束标志位
            {
                Tjc_Printf("page0.n0.val=%d",modenow);
                switch (HMI_DataBuff[HMI_RxLine-2]) 
                {
                    case 0x01:modenow = 1;break;
                    case 0x02:modenow = 2;break;
                    case 0x03:modenow = 3;break;
                    case 0x04:modenow = 4;break;
                
                }
                Tjc_Printf("page0.n0.val=%d",modenow);
                memset(HMI_DataBuff,0,sizeof(HMI_DataBuff));  //清空缓存数组
                HMI_RxLine=0;  //清空接收长度
            }
            HMI_RxBuffer[0]=0;
        }
            break;
        default:
            break;
    }
}

float Get_Data(void)
{
    uint8_t data_Start_Num = 0; // 记录数据位开始的地方
    uint8_t data_End_Num = 0; // 记录数据位结束的地方
    uint8_t data_Num = 0; // 记录数据位数
    uint8_t minus_Flag = 0; // 判断是不是负数
    float data_return = 0; // 解析得到的数据
    for(uint8_t i=0;i<200;i++) // 查找等号和感叹号的位置
    {
        if(DataBuff[i] == '=') data_Start_Num = i + 1; // +1是直接定位到数据起始位
        if(DataBuff[i] == '!')
        {
            data_End_Num = i - 1;
            break;
        }
    }
    if(DataBuff[data_Start_Num] == '-') // 如果是负数
    {
        data_Start_Num += 1; // 后移一位到数据位
        minus_Flag = 1; // 负数flag
    }
    data_Num = data_End_Num - data_Start_Num + 1;
    if(data_Num == 4) // 数据共4位
    {
        data_return = (DataBuff[data_Start_Num]-48)  + (DataBuff[data_Start_Num+2]-48)*0.1f +
                (DataBuff[data_Start_Num+3]-48)*0.01f;
    }
    else if(data_Num == 5) // 数据共5位
    {
        data_return = (DataBuff[data_Start_Num]-48)*10 + (DataBuff[data_Start_Num+1]-48) + (DataBuff[data_Start_Num+3]-48)*0.1f +
                (DataBuff[data_Start_Num+4]-48)*0.01f;
    }
    else if(data_Num == 6) // 数据共6位
    {
        data_return = (DataBuff[data_Start_Num]-48)*100 + (DataBuff[data_Start_Num+1]-48)*10 + (DataBuff[data_Start_Num+2]-48) +
                (DataBuff[data_Start_Num+4]-48)*0.1f + (DataBuff[data_Start_Num+5]-48)*0.01f;
    }
    if(minus_Flag == 1)  data_return = -data_return;
//    printf("data=%.2f\r\n",data_return);
    return data_return;
}

void USART_PID_Adjust(uint8_t Motor_n)//vofa上位机实现pid调参
{
    float data_Get = Get_Data(); // 存放接收到的数据
//    printf("data=%.2f\r\n",data_Get);
    if(Motor_n == 1)//左边电机
    {   
        if(DataBuff[0]=='P' && DataBuff[1]=='2') // 速度环P
            pid_speed1.Kp = data_Get;
        else if(DataBuff[0]=='I' && DataBuff[1]=='2') // 速度环I
            pid_speed1.Ki = data_Get;
        else if(DataBuff[0]=='D' && DataBuff[1]=='2') // 速度环D
            pid_speed1.Kd = data_Get;
        else if((DataBuff[0]=='S' && DataBuff[1]=='p') && DataBuff[2]=='e') //目标速度
            pid_speed1.target_val = data_Get;
    }

    else if(Motor_n == 2) // 右边电机
    {
        if(DataBuff[0]=='P' && DataBuff[1]=='2') // 速度环P
            pid_speed2.Kp = data_Get;
        else if(DataBuff[0]=='I' && DataBuff[1]=='2') // 速度环I
            pid_speed2.Ki = data_Get;
        else if(DataBuff[0]=='D' && DataBuff[1]=='2') // 速度环D
            pid_speed2.Kd = data_Get;
        else if((DataBuff[0]=='S' && DataBuff[1]=='p') && DataBuff[2]=='e') //目标速度
            pid_speed2.target_val = data_Get;
    }
}

void TE_SendString(char *str)
{
    while(*str != '\0')
    {
        DL_UART_Main_transmitDataBlocking(UART_0_INST,*str++);
    }
}


//printf重定向
int fputc(int c, FILE *stream) {
    DL_UART_Main_transmitDataBlocking(UART_0_INST, c);
    return c;
}

int fputs(const char *restrict s, FILE *restrict stream) {
    uint16_t i, len;
    len = strlen(s);
    for (i = 0; i < len; i++) {
    DL_UART_Main_transmitDataBlocking(UART_0_INST, s[i]);
    }
    return len;
}

int puts(const char *_ptr) {
    int count = fputs(_ptr, stdout);
    count += fputs("\n", stdout);
    return count;
}
